package com.example.ar_10;


import android.annotation.SuppressLint;
import android.content.Context;
import android.graphics.Bitmap;
import android.graphics.BitmapFactory;
import android.graphics.Color;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.net.Uri;
import android.opengl.GLES20;
import android.opengl.GLSurfaceView;
import android.os.Bundle;
import android.util.Log;
import android.util.Pair;
import android.view.View;
import android.widget.Button;
import android.widget.ImageView;
import android.widget.TextView;

import androidx.appcompat.app.AppCompatActivity;

import com.amap.api.location.AMapLocation;
import com.amap.api.location.AMapLocationClient;
import com.amap.api.location.AMapLocationClientOption;
import com.amap.api.location.AMapLocationListener;
import com.amap.api.location.AMapLocationQualityReport;
import com.bumptech.glide.Glide;
import com.bumptech.glide.RequestManager;
import com.example.ar_10.ARCoreRenderer.AugmentedImageRenderer;
import com.example.ar_10.ARCoreRenderer.BackgroundRenderer;
import com.example.ar_10.ARCoreRenderer.DisplayRotationHelper;
import com.example.ar_10.ARCoreRenderer.TrackingStateHelper;
import com.example.ar_10.myobj.PipeRenderer;
import com.example.ar_10.myobj.PositionData;
import com.google.ar.core.Anchor;
import com.google.ar.core.ArCoreApk;
import com.google.ar.core.AugmentedImage;
import com.google.ar.core.AugmentedImageDatabase;
import com.google.ar.core.Camera;
import com.google.ar.core.Config;
import com.google.ar.core.Frame;
import com.google.ar.core.Session;
import com.google.ar.core.exceptions.CameraNotAvailableException;
import com.google.ar.core.exceptions.UnavailableApkTooOldException;
import com.google.ar.core.exceptions.UnavailableArcoreNotInstalledException;
import com.google.ar.core.exceptions.UnavailableSdkTooOldException;
import com.google.ar.core.exceptions.UnavailableUserDeclinedInstallationException;

import org.opencv.android.OpenCVLoader;

import java.io.IOException;
import java.io.InputStream;
import java.util.Collection;
import java.util.HashMap;
import java.util.Map;

import javax.microedition.khronos.egl.EGLConfig;
import javax.microedition.khronos.opengles.GL10;


public class MainActivity extends AppCompatActivity implements GLSurfaceView.Renderer {

    //    private FrameLayout frameLayout;
//    private MyCamera mycamera = new MyCamera();
//    private SurfaceView mSurface;
//    private SurfaceHolder mHolder;
    private GLSurfaceView surfaceView;

    private Context mContext;
    private TextView tv_Location;
    private TextView tv_Posture;
    private TextView tv_Distance;
    private Button btnRenew;
    private Button btnInvisible;

    private static final String TAG = MainActivity.class.getSimpleName();

    //    private PlaneGlSurfaceView GLsurfaceView;
//    private PipeRenderer render;
    //声明AMapLocationClient类对象
    AMapLocationClient mLocationClient;
    //声明AMapLocationClientOption对象
    public AMapLocationClientOption mLocationOption;

    float freq_medida_Ahrs = 0;
    double timestamp_Ahrs_last = 0;
    double timestamp;
    double timestamp_Ahrs_last_update = 0;
    double deltaT_update = 0.25;   // cada 0.25 segundo actualizo la pantalla con las medidas (4Hz)
    private float currentLat;
    private float currentLon;
    private float currentAlt;
    private float currentPitch;
    private float currentRoll;
    private float currentAzimuth;
    private float[] model_view_matrix = new float[16];


    private SensorManager sensorMag;
    //保存上一次记录
    private float[] orientationAngles = new float[3];

    private PositionData positionData;
    //    private MyCamera myCamera;
    private Sensor Sensor_AHRS;

    //ARCore
    private ImageView fitToScanView;
    private RequestManager glideRequestManager;
    private boolean installRequested;
    private Session session;
    private DisplayRotationHelper displayRotationHelper;
    private final BackgroundRenderer backgroundRenderer = new BackgroundRenderer();
    private final AugmentedImageRenderer augmentedImageRenderer = new AugmentedImageRenderer();
    private final TrackingStateHelper trackingStateHelper = new TrackingStateHelper(this);
    private boolean shouldConfigureSession = false;
    // Augmented image configuration and rendering.
    // Load a single image (true) or a pre-generated image database (false).
    private final boolean useSingleImage = true;
    // Augmented image and its associated center pose anchor, keyed by index of the augmented image in
    // the
    // database.
    private final Map<Integer, Pair<AugmentedImage, Anchor>> augmentedImageMap = new HashMap<>();

    static {
        if (!OpenCVLoader.initDebug())
            Log.d("ERROR", "Unable to load OpenCV");
        else
            Log.d("SUCCESS", "OpenCV loaded");
    }

    @Override
    protected void onCreate(Bundle savedInstanceState) {

        super.onCreate(savedInstanceState);
        // 设置界面没有标题
//        requestWindowFeature(Window.FEATURE_NO_TITLE);
        // 切换到主界面
        setContentView(R.layout.activity_main);
        // 设置为横屏模式
//        setRequestedOrientation(ActivityInfo.SCREEN_ORIENTATION_);
        // 设置为全屏
//        getWindow().setFlags(WindowManager.LayoutParams.FLAG_FULLSCREEN, WindowManager.LayoutParams.FLAG_FULLSCREEN);

        mContext = this;
        positionData = new PositionData(this, MainActivity.this);
//        myCamera = new MyCamera(this, MainActivity.this);

//        frameLayout = findViewById(R.id.frameView);
        tv_Posture = findViewById(R.id.tv_Posture);
        tv_Location = findViewById(R.id.tv_Location);
        btnRenew = findViewById(R.id.btnRenew);
        btnInvisible = findViewById(R.id.btnInvisible);
        tv_Distance = findViewById(R.id.tv_Distabce);
        surfaceView = findViewById(R.id.surfaceview);

        //初始化传感器
        sensorMag = (SensorManager) getSystemService(SENSOR_SERVICE);
        Sensor_AHRS = sensorMag.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
        sensorMag.registerListener(sensorLis, sensorMag.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR), SensorManager.SENSOR_DELAY_NORMAL);

//        sensorMag.registerListener(sensorLis, sensorMag.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_NORMAL);
//        sensorMag.registerListener(sensorLis, sensorMag.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_NORMAL);


        btnRenew.setOnClickListener(v -> {
            onPause();
            onStart();
            Log.e(TAG, "renew");
        });

        btnInvisible.setOnClickListener(v -> {
            textViewInvisible();
            Log.e(TAG, "点击按钮");
        });

        //获取当前位置信息
        getCurrentLocationLatLng();

//        /**
//         * 摄像头调用图层
//         */
//        mSurface = new SurfaceView(this);
//        mHolder = mSurface.getHolder();
//        mHolder.addCallback(new Callback() {
//            @Override
//            public void surfaceDestroyed(SurfaceHolder holder) {
//                mycamera.stopCamera();
//            }
//
//            @Override
//            public void surfaceCreated(SurfaceHolder holder) {
//                //初始化摄像头
//                mycamera.openCam(holder, mContext);
//                mycamera.everyTime();
//                model_view_matrix = mycamera.getModel_view_matrix();
//            }
//
//            @Override
//            public void surfaceChanged(SurfaceHolder holder, int format, int width, int height) {
//                mycamera.pingMuChange();
//            }
//        });
//        mHolder.setType(SurfaceHolder.SURFACE_TYPE_PUSH_BUFFERS);
//        frameLayout.addView(mSurface, new LayoutParams(LayoutParams.FILL_PARENT, LayoutParams.FILL_PARENT));
//        Log.d(TAG, "摄像头调用图层");
//
//        /**
//         * 3D图像调用
//         */
//        /**
//         * Set the desired PixelFormat of the surface. The default is OPAQUE. When working with a SurfaceView,
//         * this must be called from the same thread running the SurfaceView's window.
//         */
//        // Set up renderer.
//
//        GLsurfaceView = new PlaneGlSurfaceView(this);
//        render = new PipeRenderer(GLsurfaceView, this);
////        GLsurfaceView.setOnTouchListener(render.getTouchEventListener());
//        frameLayout.addView(GLsurfaceView, new LayoutParams(LayoutParams.FILL_PARENT, LayoutParams.FILL_PARENT));
//        Log.d(TAG, "3D图像调用");

        // Set up renderer.
        surfaceView.setPreserveEGLContextOnPause(true);
        surfaceView.setEGLContextClientVersion(2);
        surfaceView.setEGLConfigChooser(8, 8, 8, 8, 16, 0); // Alpha used for plane blending.
        surfaceView.setRenderer(this);
        surfaceView.setRenderMode(GLSurfaceView.RENDERMODE_CONTINUOUSLY);
        surfaceView.setWillNotDraw(false);

        displayRotationHelper = new DisplayRotationHelper(/*context=*/ this);


        fitToScanView = findViewById(R.id.image_view_fit_to_scan);
        glideRequestManager = Glide.with(this);
        glideRequestManager
                .load(Uri.parse("file:///android_asset/fit_to_scan.png"))
                .into(fitToScanView);
        installRequested = false;
    }


    /**
     * 手机定位
     * 初始化定位并设置定位回调监听
     */
    private void getCurrentLocationLatLng() {
        //初始化定位
        mLocationClient = new AMapLocationClient(getApplicationContext());
        //设置定位回调监听
        mLocationClient.setLocationListener(mLocationListener);
        //初始化AMapLocationClientOption对象
        mLocationOption = new AMapLocationClientOption();
        // 同时使用网络定位和GPS定位,优先返回最高精度的定位结果,以及对应的地址描述信息
        mLocationOption.setLocationMode(AMapLocationClientOption.AMapLocationMode.Hight_Accuracy);
        //设置定位间隔,单位毫秒,默认为2000ms，最低1000ms。默认连续定位 切最低时间间隔为1000ms
        mLocationOption.setInterval(1000);
        //给定位客户端对象设置定位参数
        mLocationClient.setLocationOption(mLocationOption);
    }


    /**
     * 定位回调监听器
     */
    public AMapLocationListener mLocationListener = new AMapLocationListener() {
        @SuppressLint("SetTextI18n")
        @Override
        public void onLocationChanged(AMapLocation amapLocation) {

            if (amapLocation != null) {
                if (amapLocation.getErrorCode() == 0) {
                    AMapLocationQualityReport aMapLocationQualityReport = new AMapLocationQualityReport();
                    //定位成功回调信息，设置相关消息
                    //获取当前定位结果来源，如网络定位结果，详见定位类型表
                    amapLocation.getLocationType();
                    //获取纬度
                    currentLat = (float) amapLocation.getLatitude();
                    //获取经度
                    currentLon = (float) amapLocation.getLongitude();

                    currentAlt = (float) amapLocation.getAltitude();
                    //获取当前城市位置
                    tv_Location.setBackgroundColor(Color.argb(155, 255, 255, 255));
                    tv_Location.setText("纬度：\t" + currentLat + "\n经度：\t" + currentLon + "\n" +
                            "海拔：" + currentAlt + "m\n" +
                            LocationType(amapLocation.getLocationType()) + "\n" +
                            "定位精度：" + amapLocation.getAccuracy() + "m"
                    );//详细信息


                } else {
                    //显示错误信息ErrCode是错误码，errInfo是错误信息，详见错误码表。
                    Log.e("AmapError", "location Error, ErrCode:"
                            + amapLocation.getErrorCode() + ", errInfo:"
                            + amapLocation.getErrorInfo());
                }


            }
            float distance = PipeRenderer.getDistance_V4M();
            float[] modelPos = PositionData.xyzModel();
            float[] xyzView = PositionData.xyzCamera();

            tv_Distance.setBackgroundColor(Color.argb(155, 255, 255, 255));
            tv_Distance.setText("距目标模型还剩" + distance + "米" + "\n模型坐标 \nX:" + modelPos[0] + "\nY:" + modelPos[1] + "\nZ:" + modelPos[2]
                    + "\n相机坐标\nX:" + xyzView[0] + "\nY:" + xyzView[1] + "\nZ:" + xyzView[2]);

        }
    };

    /**
     * 定位类型
     *
     * @param typeNumber
     * @return
     */
    public String LocationType(int typeNumber) {
        String type = null;
        switch (typeNumber) {
            case 1:
                type = "卫星定位结果";
                break;
            case 2:
            case 3:
                type = "前次定位结果";
                break;
            case 4:
                type = "缓存定位结果";
                break;
            case 5:
                type = "WIFI定位结果";
                break;
            case 6:
                type = "基站定位结果";
                break;
            case 7:
                type = "LOCATION_TYPE_AMAP";
                break;
            case 8:
                type = "离线定位结果";
                break;
            case 9:
                type = "最后位置缓存";
                break;
            case 10:
                type = "LOCATION_COMPENSATION";
                break;
        }
        return type;
    }

    /**
     * 传感器获取手机姿态
     */

    private final SensorEventListener sensorLis = new SensorEventListener() {
        @SuppressLint("SetTextI18n")
        @Override
        public void onSensorChanged(SensorEvent event) {

            long SensorTimestamp_ns_raw = event.timestamp;      // in nano seconds
            double SensorTimestamp = ((double) (SensorTimestamp_ns_raw)) * 1E-9;  // de nano_s a segundos

            if (event.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR) {
                freq_medida_Ahrs = (float) (0.9 * freq_medida_Ahrs + 0.1 / (SensorTimestamp - timestamp_Ahrs_last));
                timestamp_Ahrs_last = SensorTimestamp;
                // Many sensors return 3 values, one for each axis.
                float[] AHRS_data = event.values;
                float[] Rot_b_g = {1, 0, 0, 0, 1, 0, 0, 0, 1};
                try {
                    SensorManager.getRotationMatrixFromVector(Rot_b_g, AHRS_data);
                } catch (IllegalArgumentException e) {
                    if (AHRS_data.length > 3) {
                        // Note 3 bug
                        float[] newVector = new float[]{
                                AHRS_data[0],
                                AHRS_data[1],
                                AHRS_data[2]
                        };
                        SensorManager.getRotationMatrixFromVector(Rot_b_g, newVector);
                    }
                }

                orientationAngles = SensorManager.getOrientation(Rot_b_g, orientationAngles);  // En radianes pero lo da en WND (West/North/Down)
                orientationAngles[0] = orientationAngles[0];
                orientationAngles[1] = orientationAngles[1]; // Paso de WND a de nuevo ENU (que es donde quiero trabajar)
                double PI = 3.14159265358979323846;
                currentAzimuth = (float) (orientationAngles[0] * 180 / PI);  // Yaw, ejeZ en grados
                currentPitch = (float) (orientationAngles[1] * 180 / PI);  // Pitch, eje X
                currentRoll = (float) (orientationAngles[2] * 180 / PI);  // Roll, eje Y

                if (currentAzimuth < 0) {
                    currentAzimuth += 360;
                }

                tv_Posture.setBackgroundColor(Color.argb(155, 255, 255, 255));
                tv_Posture.setBackgroundColor(Color.argb(155, 255, 255, 255));
                tv_Posture.setText("Pitch(X):" + "\t" + currentPitch + "\tdegress\n" + "Roll(Y):" + "\t" + currentRoll
                        + "\tdegress\n" + "Azimuth(Z):" + "\t" + currentAzimuth);
                timestamp_Ahrs_last_update = timestamp;
//                }
            }

        }

        @Override
        public void onAccuracyChanged(Sensor sensor, int accuracy) {
        }
    };

    /**
     * 显示全屏设置
     *
     * @param hasFocus
     */
//    @Override
//    public void onWindowFocusChanged(boolean hasFocus) {
//        super.onWindowFocusChanged(hasFocus);
//        FullScreenHelper.setFullScreenOnWindowFocusChanged(this, hasFocus);
//    }


    /**
     * 隐藏按钮
     */
    private void textViewInvisible() {
        if (btnInvisible.getText().toString().equals("隐藏")) {
            tv_Location.setVisibility(View.GONE);
            btnRenew.setVisibility(View.GONE);
            tv_Posture.setVisibility(View.GONE);
            tv_Distance.setVisibility(View.GONE);
            btnInvisible.setText("显示");
        } else {
            tv_Location.setVisibility(View.VISIBLE);
            btnRenew.setVisibility(View.VISIBLE);
            tv_Posture.setVisibility(View.VISIBLE);
            tv_Distance.setVisibility(View.VISIBLE);
            btnInvisible.setText("隐藏");
        }
    }

    @Override
    protected void onStart() {
        super.onStart();
        if (mLocationClient != null) {
            // 启动定位
            mLocationClient.startLocation();
        }
        sensorMag.registerListener(sensorLis, sensorMag.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_NORMAL);
    }

    @Override
    protected void onResume() {

        super.onResume();

        if (session == null) {
            Exception exception = null;
            String message = null;
            try {
                switch (ArCoreApk.getInstance().requestInstall(this, !installRequested)) {
                    case INSTALL_REQUESTED:
                        installRequested = true;
                        return;
                    case INSTALLED:
                        break;
                }
                session = new Session(/* context = */ this);
            } catch (UnavailableArcoreNotInstalledException
                    | UnavailableUserDeclinedInstallationException e) {
                message = "Please install ARCore";
                exception = e;
            } catch (UnavailableApkTooOldException e) {
                message = "Please update ARCore";
                exception = e;
            } catch (UnavailableSdkTooOldException e) {
                message = "Please update this app";
                exception = e;
            } catch (Exception e) {
                message = "This device does not support AR";
                exception = e;
            }

            if (message != null) {
                Log.e(TAG, "Exception creating session", exception);
                return;
            }
            shouldConfigureSession = true;
        }

        if (shouldConfigureSession) {
            configureSession();
            shouldConfigureSession = false;
        }

        // Note that order matters - see the note in onPause(), the reverse applies here.
        try {
            session.resume();
        } catch (CameraNotAvailableException e) {
            Log.e(TAG, "Camera not available. Try restarting the app");
            session = null;
            return;
        }
        surfaceView.onResume();
        displayRotationHelper.onResume();
        fitToScanView.setVisibility(View.VISIBLE);
    }

    @Override
    protected void onPause() {
        super.onPause();
        if (mLocationClient != null) {
            //停止定位
            mLocationClient.stopLocation();
        }
        sensorMag.unregisterListener(sensorLis);
        if (session != null) {
            displayRotationHelper.onPause();
            surfaceView.onPause();
            session.pause();
        }
    }

    @Override
    protected void onDestroy() {
        super.onDestroy();
        if (mLocationClient != null) {
            //销毁定位客户端
            mLocationClient.onDestroy();
        }

        if (session != null) {
            session.close();
            session = null;
        }
    }


    @Override
    public void onSurfaceCreated(GL10 gl, EGLConfig config) {
        GLES20.glClearColor(0.1f, 0.1f, 0.1f, 1.0f);

        // Prepare the rendering objects. This involves reading shaders, so may throw an IOException.
        try {
            // Create the texture and pass it to ARCore session to be filled during update().
            backgroundRenderer.createOnGlThread(/*context=*/ this);
            augmentedImageRenderer.createOnGlThread(/*context=*/ this);
        } catch (IOException e) {
            Log.e(TAG, "Failed to read an asset file", e);
        }
    }

    @Override
    public void onSurfaceChanged(GL10 gl, int width, int height) {
        displayRotationHelper.onSurfaceChanged(width, height);
        GLES20.glViewport(0, 0, width, height);
    }

    @Override
    public void onDrawFrame(GL10 gl) {

        // Clear screen to notify driver it should not load any pixels from previous frame.
        GLES20.glClear(GLES20.GL_COLOR_BUFFER_BIT | GLES20.GL_DEPTH_BUFFER_BIT);

        if (session == null) {
            return;
        }
        // Notify ARCore session that the view size changed so that the perspective matrix and
        // the video background can be properly adjusted.
        displayRotationHelper.updateSessionIfNeeded(session);

        try {
            session.setCameraTextureName(backgroundRenderer.getTextureId());

            // Obtain the current frame from ARSession. When the configuration is set to
            // UpdateMode.BLOCKING (it is by default), this will throttle the rendering to the
            // camera framerate.
            Frame frame = session.update();
            Camera camera = frame.getCamera();

            // Keep the screen unlocked while tracking, but allow it to lock when tracking stops.
            trackingStateHelper.updateKeepScreenOnFlag(camera.getTrackingState());

            // If frame is ready, render camera preview image to the GL surface.
            backgroundRenderer.draw(frame);

            // Get projection matrix.
            float[] projmtx = new float[16];
            camera.getProjectionMatrix(projmtx, 0, 0.1f, 100.0f);

            // Get camera matrix and draw.
            float[] viewmtx = new float[16];
            camera.getViewMatrix(viewmtx, 0);

            // Compute lighting from average intensity of the image.
            final float[] colorCorrectionRgba = new float[4];
            frame.getLightEstimate().getColorCorrection(colorCorrectionRgba, 0);

            // Visualize augmented images.
            drawAugmentedImages(frame, projmtx, viewmtx, colorCorrectionRgba);
        } catch (Throwable t) {
            // Avoid crashing the application due to unhandled exceptions.
            Log.e(TAG, "Exception on the OpenGL thread", t);
        }
    }// onDrawFrame finish

    private void configureSession() {
        Config config = new Config(session);
        config.setFocusMode(Config.FocusMode.AUTO);
        if (!setupAugmentedImageDatabase(config)) {
            Log.e(TAG, "Could not setup augmented image database");
        }
        session.configure(config);
    }

    private void drawAugmentedImages(
            Frame frame, float[] projmtx, float[] viewmtx, float[] colorCorrectionRgba) {
        Collection<AugmentedImage> updatedAugmentedImages =
                frame.getUpdatedTrackables(AugmentedImage.class);

        // Iterate to update augmentedImageMap, remove elements we cannot draw.
        for (AugmentedImage augmentedImage : updatedAugmentedImages) {
            switch (augmentedImage.getTrackingState()) {
                case PAUSED:
                    // When an image is in PAUSED state, but the camera is not PAUSED, it has been detected,
                    // but not yet tracked.
                    Log.d(TAG, "Detected Image " + augmentedImage.getIndex() + "%");
                    break;

                case TRACKING:
                    // Have to switch to UI Thread to update View.
                    this.runOnUiThread(
                            new Runnable() {
                                @Override
                                public void run() {
                                    fitToScanView.setVisibility(View.GONE);
                                }
                            });

                    // Create a new anchor for newly found images.
                    if (!augmentedImageMap.containsKey(augmentedImage.getIndex())) {
                        Anchor centerPoseAnchor = augmentedImage.createAnchor(augmentedImage.getCenterPose());
                        augmentedImageMap.put(
                                augmentedImage.getIndex(), Pair.create(augmentedImage, centerPoseAnchor));
                    }
                    Log.d(TAG, "getName: " + augmentedImage.getName());
                    break;

                case STOPPED:
                    augmentedImageMap.remove(augmentedImage.getIndex());
                    break;

                default:
                    break;
            }
        }

        // Draw all images in augmentedImageMap
        for (Pair<AugmentedImage, Anchor> pair : augmentedImageMap.values()) {
            AugmentedImage augmentedImage = pair.first;
            Anchor centerAnchor = augmentedImageMap.get(augmentedImage.getIndex()).second;
            switch (augmentedImage.getTrackingState()) {
                case TRACKING:
                    augmentedImageRenderer.draw(
                            viewmtx, projmtx, augmentedImage, centerAnchor, colorCorrectionRgba);
                    break;
                default:
                    break;
            }
        }
    }

    private boolean setupAugmentedImageDatabase(Config config) {

        AugmentedImageDatabase augmentedImageDatabase;

        // There are two ways to configure an AugmentedImageDatabase:
        // 1. Add Bitmap to DB directly
        // 2. Load a pre-built AugmentedImageDatabase
        // Option 2) has
        // * shorter setup time
        // * doesn't require images to be packaged in apk.
        if (useSingleImage) {
            Bitmap augmentedImageBitmap = loadAugmentedImageBitmap();
            if (augmentedImageBitmap == null) {
                return false;
            }
            augmentedImageDatabase = new AugmentedImageDatabase(session);
            augmentedImageDatabase.addImage("image_name", augmentedImageBitmap);
            // If the physical size of the image is known, you can instead use:
            //     augmentedImageDatabase.addImage("image_name", augmentedImageBitmap, widthInMeters);
            // This will improve the initial detection speed. ARCore will still actively estimate the
            // physical size of the image as it is viewed from multiple viewpoints.
        } else {
            // This is an alternative way to initialize an AugmentedImageDatabase instance,
            // load a pre-existing augmented image database.
            try (InputStream is = getAssets().open("sample_database.imgdb")) {
                augmentedImageDatabase = AugmentedImageDatabase.deserialize(session, is);
            } catch (IOException e) {
                Log.e(TAG, "IO exception loading augmented image database.", e);
                return false;
            }
        }
        config.setAugmentedImageDatabase(augmentedImageDatabase);
        return true;
    }

    private Bitmap loadAugmentedImageBitmap() {
        try (InputStream is = getAssets().open("Test.jpeg")) {
            return BitmapFactory.decodeStream(is);
        } catch (IOException e) {
            Log.e(TAG, "IO exception loading augmented image bitmap.", e);
        }
        return null;
    }

    public float getCurrentLat() {
        return currentLat;
    }

    public float getCurrentLon() {
        return currentLon;
    }

    public float getCurrentAlt() {
        return currentAlt;
    }

    public float getCurrentPitch() {  //单位:弧度
        return orientationAngles[1];
    }

    public float getCurrentRoll() {
        return orientationAngles[2];
    }

    public float getCurrentAzimuth() {
        if (orientationAngles[0] < 0) {
            orientationAngles[0] += 2 * Math.PI;
        }
        return orientationAngles[0];
    }

    public float[] getModel_view_matrix() {
        return model_view_matrix;
    }
}